# -*- coding:utf-8 -*-
import json, codecs, time, os
from glob import glob

import numpy as np
from PIL import Image
from tqdm import tqdm

# https://medium.com/yodayoda/from-depth-map-to-point-cloud-7473721d3f

# TODO : 这是第一种方法直接使用遍历点数，计算出对应的 XYZ
# focal_X = 7.070493e+02
# focal_Y = 7.070493e+02
# c_X = 6.040814e+02
# c_Y = 1.805066e+02
#
# def convert_from_uvd(u, v, d,fx,fy,cx,cy):
# 	# d *= self.pxToMetre
# 	x_over_z = (cx - u) / fx
# 	y_over_z = (cy - v) / fy
# 	z = d / np.sqrt(1. + x_over_z**2 + y_over_z**2)
# 	x = x_over_z * z
# 	y = y_over_z * z
# 	return x, y, z
#
# xyz_list = []
#
# img = Image.open("./data/mono-data/inference/0000000042.png")
# img = np.array(img) / 256.
# # print(img.shape) # (375, 1242)
# # height, width = 375, 1242
# for i in tqdm(range(img.shape[0])):
# 	for j in range(img.shape[1]):
# 		# if img[i, j] >0.:
# 			x,y,z = convert_from_uvd(i,j, img[i,j],
# 									 focal_X,
# 									 focal_Y,
# 									 c_X,
# 									 c_Y)
# 			xyz_list.append([x,y,z])
# print(xyz_list)
# import open3d as o3d
#
# pcd = o3d.geometry.PointCloud()
# pcd.points = o3d.utility.Vector3dVector(xyz_list)
# vis = o3d.visualization.Visualizer()
# vis.create_window()
# vis.add_geometry(pcd)
# view_ctl = vis.get_view_control()
# view_ctl.set_front((1, 0, 1))
# view_ctl.set_up((0, 0, 1))
# view_ctl.set_lookat(pcd.get_center())
# vis.run()
# # vis.destroy_window()



# # TODO : 使用矩阵完成点位 3D 转换
import math
from scipy import ndimage

def edges(d):
	dx = ndimage.sobel(d, 0)  # horizontal derivative
	dy = ndimage.sobel(d, 1)  # vertical derivative
	return np.abs(dx) + np.abs(dy)

class PointCloudHelper():
	def __init__(self):
		self.width = 1242
		self.height = 375
		self.focal_X = 7.070493e+02
		self.focal_Y = 7.070493e+02
		self.c_X = 6.040814e+02
		self.c_Y = 1.805066e+02


	def worldCoords(self, depth_map):
		xx, yy = np.tile(range(self.width), self.height).reshape(self.height, self.width), \
				 np.repeat(range(self.height), self.width).reshape(self.height, self.width)
		x_over_z = (self.c_X - xx) / self.focal_X
		y_over_z = (self.c_Y - yy) / self.focal_Y
		z_ndarray = depth_map / np.sqrt(1 + x_over_z ** 2 + y_over_z ** 2)
		x_ndarray = x_over_z * z_ndarray
		y_ndarray = y_over_z * z_ndarray

		xyz_list = np.stack([x_ndarray, z_ndarray, y_ndarray], -1).reshape(-1, 3).tolist()
		return xyz_list


helper = PointCloudHelper()
start_time = time.time()
dense_depth_PATH = "/home/liuyang/Desktop/08_ice_tasks/11_display_3d/data/mono-data/dense_depth/0000000042.png"

depth_map = np.array(Image.open(dense_depth_PATH)) / 256
xyz_list = helper.worldCoords(depth_map)
end_time = time.time()
print(end_time - start_time)
time.sleep(100)
import open3d as o3d

pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz_list)
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
view_ctl = vis.get_view_control()
view_ctl.set_front((1, 0, 1))
view_ctl.set_up((0, 0, 1))
view_ctl.set_lookat(pcd.get_center())
vis.run()
# vis.destroy_window()
